#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/ioctl.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <linux/types.h>
#include <linux/videodev2.h>
#include "usb_v4l2.h"
//#include <glib.h>
#define YUV0	        "/usr/image_yuv0.yuv"
#define YUV1	        "/usr/image_yuv1.yuv"
#define YUV2	        "/usr/image_yuv2.yuv"
#define YUV3	        "/usr/image_yuv3.yuv"
#define YUV4	        "/usr/image_yuv4.yuv"

static   int      fd;
static   struct   v4l2_capability   cap;
struct v4l2_fmtdesc fmtdesc;
struct v4l2_format fmt,fmtack;

struct v4l2_streamparm setfps;  
struct v4l2_requestbuffers req;
struct v4l2_buffer buf;
enum v4l2_buf_type type;
unsigned char frame_buffer[IMAGEWIDTH*IMAGEHEIGHT*3];
unsigned int index_flag;
unsigned char * yuv_buffer_pointer;
unsigned int len;

//閿熻璁规嫹閿熸枻鎷峰钩閿熸枻鎷�
struct v4l2_control control_s;

/*****************************************************************************
  Function:     閿熸枻鎷峰閿熸枻鎷稸4l2
  Description:  1閿熸枻鎷烽敓鏂ゆ嫹濮嬮敓鏂ゆ嫹V4l2閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熷�熷閿熸枻鎷�
				2閿熸枻鎷烽敓鏂ゆ嫹video_capability閿熸枻鎷烽敓鏂ゆ嫹鎭敓鏂ゆ嫹
				閫氶敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹IOCTL閿熸枻鎷烽敓鏂ゆ嫹閿熼叺鎺ュ尅鎷烽敓鏂ゆ嫹閿熸枻鎷稸IDIOC_QUERYCAP閿熸枻鎷疯閿熸枻鎷烽敓鏂ゆ嫹澶撮敓鏂ゆ嫹閿熸枻鎷锋伅閿熸枻鎷�
				閿熺粨鏋勯敓鏂ゆ嫹v4l2_capability閿熸枻鎷烽敓鍙府鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹driver閿熸枻鎷穋ard閿熸枻鎷穊us_info閿熸枻鎷�
				version閿熺殕纭锋嫹閿熸枻鎷烽敓鏂ゆ嫹capabilities閿熸枻鎷�
				3閿熸枻鎷烽敓鍙拝鎷烽敓鏂ゆ嫹閿熸枻鎷峰ご閿熸枻鎷锋敮閿熸枻鎷烽敓鏂ゆ嫹閿熸埅闈╂嫹寮忛敓鏂ゆ嫹YUV2閿熸枻鎷稭JPEG閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹鍥鹃敓鏂ゆ嫹閿熺粸鏂ゆ嫹閿熸枻鎷烽敓鐙＄鎷锋潐閿熻鈽呮嫹鍓╅敓锟�
  Input:        none
  Output:       none
  Return:       none
  Author:       cc
  Modify:
*****************************************************************************/
int init_v4l2(void)
{
	//opendev
	if ((fd = open(FILE_VIDEO, O_RDWR)) == -1) 
	{
		printf("Error opening V4L interface\n");
		return (FALSE);
	}

	//query cap
	if (ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) 
	{
		printf("Error opening device %s: unable to query device.\n",FILE_VIDEO);
		return (FALSE);
	}
	else
	{
     	printf("driver:\t\t%s\n",cap.driver);
     	printf("card:\t\t%s\n",cap.card);
     	printf("bus_info:\t%s\n",cap.bus_info);
     	printf("version:\t%d\n",cap.version);
     	printf("capabilities:\t%x\n",cap.capabilities);
     	
     	if ((cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) == V4L2_CAP_VIDEO_CAPTURE) 
     	{
			printf("Device %s: supports capture.\n",FILE_VIDEO);
		}

		if ((cap.capabilities & V4L2_CAP_STREAMING) == V4L2_CAP_STREAMING) 
		{
			printf("Device %s: supports streaming.\n",FILE_VIDEO);
		}
	} 
	
	//emu all support fmt
	fmtdesc.index=0;
	fmtdesc.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
	printf("Support format:\n");
	while(ioctl(fd,VIDIOC_ENUM_FMT,&fmtdesc)!=-1)
	{
		printf("\t%d.%s\n",fmtdesc.index+1,fmtdesc.description);
		fmtdesc.index++;
	}
	
    //set fmt
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
//	fmt.type = V4L2_CAP_STREAMING;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
	fmt.fmt.pix.height = IMAGEHEIGHT;
	fmt.fmt.pix.width = IMAGEWIDTH;
	fmt.fmt.pix.bytesperline = IMAGEWIDTH*2;
	fmt.fmt.pix.sizeimage = IMAGEHEIGHT*IMAGEWIDTH*2;
	fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
	
	if(ioctl(fd, VIDIOC_S_FMT, &fmt) == -1)
	{
		printf("Unable to set format\n");
		return FALSE;
	} 	
	if(ioctl(fd, VIDIOC_G_FMT, &fmt) == -1)
	{
		printf("Unable to get format\n");
		return FALSE;
	} 
	{
     	printf("fmt.type:\t\t%d\n",fmt.type);
     	printf("pix.pixelformat:\t%c%c%c%c\n",fmt.fmt.pix.pixelformat & 0xFF, (fmt.fmt.pix.pixelformat >> 8) & 0xFF,(fmt.fmt.pix.pixelformat >> 16) & 0xFF, (fmt.fmt.pix.pixelformat >> 24) & 0xFF);
     	printf("pix.height:\t\t%d\n",fmt.fmt.pix.height);
     	printf("pix.width:\t\t%d\n",fmt.fmt.pix.width);
     	printf("pix.bytesperline:\t\t%d\n",fmt.fmt.pix.bytesperline);
     	printf("pix.sizeimage:\t\t%d\n",fmt.fmt.pix.sizeimage);
     	printf("pix.field:\t\t%d\n",fmt.fmt.pix.field);
	}
	//set fps
	setfps.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
//	setfps.type = V4L2_CAP_STREAMING;
	setfps.parm.capture.timeperframe.numerator = 1;
	setfps.parm.capture.timeperframe.denominator = FRAME_RATE;
	
    if(ioctl(fd, VIDIOC_S_PARM, &setfps) == -1)
    {
        printf("Unable to set frame rate\n");
        return FALSE;
    }
    else
    {
        printf("set fps OK!\n");
    }

    if(ioctl(fd, VIDIOC_G_PARM, &setfps) == -1)
    {
        printf("Unable to get frame rate\n");
        return FALSE;
    }
    else
    {
        printf("get fps OK!\n");
        printf("timeperframe.numerator:\t%d\n",setfps.parm.capture.timeperframe.numerator);
        printf("timeperframe.denominator:\t%d\n",setfps.parm.capture.timeperframe.denominator);

    }

	printf("init %s \t[OK]\n",FILE_VIDEO);
	    
	return TRUE;
}



/*****************************************************************************
  Function:     鍥鹃敓鏂ゆ嫹鏉夐敓锟�
  Description:  1閿熸枻鎷烽敓鏂ゆ嫹閿熻缂撻敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹
				2閿熸枻鎷烽敓鏂ゆ嫹鍙栨瘡閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熻緝顫嫹閿熸枻鎷烽敓绲ap閿熸枻鎷烽敓鐭紮鎷烽敓绉搁棿銆傞敓鏂ゆ嫹閿熸枻鎷峰硜鐧告嫹婢癸拷
  Input:        none
  Output:       none
  Return:       none
  Author:       cc
  Modify:
*****************************************************************************/
int v4l2_grab(void)
{
	unsigned int n_buffers;
	
	//request for 4 buffers 
	req.count=4;
	req.type=V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory=V4L2_MEMORY_MMAP;
	if(ioctl(fd,VIDIOC_REQBUFS,&req)==-1)
	{
		printf("request for buffers error\n");
	}

	//mmap for buffers
	buffers = malloc(req.count*sizeof (*buffers));
	if (!buffers) 
	{
		printf ("Out of memory\n");
		return(FALSE);
	}
	
	for (n_buffers = 0; n_buffers < req.count; n_buffers++) 
	{
		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index = n_buffers;
		//query buffers
		if (ioctl (fd, VIDIOC_QUERYBUF, &buf) == -1)
		{
			printf("query buffer error\n");
			return(FALSE);
		}

		buffers[n_buffers].length = buf.length;
		//map
		buffers[n_buffers].start = mmap(NULL,buf.length,PROT_READ |PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
		if (buffers[n_buffers].start == MAP_FAILED)
		{
			printf("buffer map error\n");
			return(FALSE);
		}
	}
		
	//queue
	for (n_buffers = 0; n_buffers < req.count; n_buffers++)
	{
		buf.index = n_buffers;
		ioctl(fd, VIDIOC_QBUF, &buf);
	} 
	
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ioctl (fd, VIDIOC_STREAMON, &type); // 閿熸枻鎷峰閿熺即纭锋嫹
//
//	ioctl(fd, VIDIOC_DQBUF, &buf);  //閿熸枻鎷峰彇閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷�
//
//        printf("grab yuyv OK\n");
	printf("grab is ok\n" );
	return(TRUE);
}


/*****************************************************************************
  Function:     鍋滄閿熸枻鎷烽閿熺即纭锋嫹
  Description:  閿熸埅鎲嬫嫹閿熷�熷閿熸枻鎷峰仠姝㈤敓鏂ゆ嫹棰戦敓缂寸》鎷�
  Input:        none
  Output:       none
  Return:       none
  Author:       cc
  Modify:
*****************************************************************************/
int close_v4l2(void)
{
     if(fd != -1) 
     {
         close(fd);
         return (TRUE);
     }
     return (FALSE);
}


/*****************************************************************************
  Function:     鍙栭敓鏂ゆ嫹鍥鹃敓鏂ゆ嫹
  Description:  鍙栭敓鏂ゆ嫹閿熸枻鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓绐栨拝鎷烽敓鏂ゆ嫹閿熸枻鎷烽敓渚ヤ紮鎷烽敓鑺ワ紝
				閿熸枻鎷烽閿熸枻鎷烽敓鎹疯揪鎷疯閿熻娇浼欐嫹閿熸枻鎷烽敓绲檜ffers[n_buffers].start閿熶茎纰夋嫹鍧�閿熸枻鎷烽敓鏂ゆ嫹
  Input:        frame_buf閿熸枻鎷穕en
  Output:       none
  Return:       none
  Author:       cc
  Modify:
*****************************************************************************/

////int get_frame(void **frame_buf, size_t* len,unsigned short yuvDatayuvData[CAM_HSIZE * CAM_VSIZE])
//int get_frame(unsigned short yuyvData[CAM_HSIZE * CAM_VSIZE*2])
//{
//	unsigned char * frame_buf;
//    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
//    buf.memory = V4L2_MEMORY_MMAP;
//
//    if(ioctl(fd, VIDIOC_DQBUF, &buf) == -1)
//    {
//        return FALSE;
//    }
//
//    frame_buf = buffers[buf.index].start;
////    *len = buffers[buf.index].length;
//    index_flag = buf.index;
////    printf("%d\n",buffers[buf.index].length);
//
//	memcpy2usb(/*frame,*/ frame_buf, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);
//
//
//
////	unsigned int i,j,index=0;
////    unsigned char y1,y2,u,v;
////	unsigned short yu_tmp,yv_tmp;
////	for( i=0; i<IMAGEHEIGHT; i++ )
////	{
////		for( j=0; j<IMAGEWIDTH/2; j++ )
////		{
////    		y1 = *( frame_buf + (i*IMAGEWIDTH/2+j)*4);
////    		u  = *( frame_buf + (i*IMAGEWIDTH/2+j)*4 + 1);
////    		y2 = *( frame_buf + (i*IMAGEWIDTH/2+j)*4 + 2);
////    		v  = *( frame_buf + (i*IMAGEWIDTH/2+j)*4 + 3);
////
//////    		yu_tmp = y1+(unsigned short)u<<8;
////    		yu_tmp = u+(unsigned short)y1<<8;
////    		yuyvData[index++]=yu_tmp;
//////    		yv_tmp = y2+(unsigned short)v<<8;
////    		yv_tmp = v+(unsigned short)y2<<8;
////    		yuyvData[index++]=yv_tmp;
////		}
//
////	}


int get_frame(unsigned char yuyvData[CAM_HSIZE * CAM_VSIZE*CAM_PIXELLENGTH])
{
	unsigned char * frame_buf;
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    if(ioctl(fd, VIDIOC_DQBUF, &buf) == -1)
    {
        return FALSE;
    }

    frame_buf = buffers[buf.index].start;
//    *len = buffers[buf.index].length;
    index_flag = buf.index;
//    printf("%d\n",buffers[buf.index].length);

//	memcpy2usb(frame, frame_buf, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);



	unsigned int i,j,index=0;
    unsigned char y1,y2,u,v;
	unsigned short yu_tmp,yv_tmp;
	for( i=0; i<IMAGEHEIGHT; i++ )
	{
		for( j=0; j<IMAGEWIDTH/2; j++ )
		{
    		y1 = *( frame_buf + (i*IMAGEWIDTH/2+j)*4);
    		u  = *( frame_buf + (i*IMAGEWIDTH/2+j)*4 + 1);
    		y2 = *( frame_buf + (i*IMAGEWIDTH/2+j)*4 + 2);
    		v  = *( frame_buf + (i*IMAGEWIDTH/2+j)*4 + 3);

////    		yu_tmp = y1+(unsigned short)u<<8;
//    		yu_tmp = u+(unsigned short)y1<<8;
//    		yuyvData[index++]=yu_tmp;
////    		yv_tmp = y2+(unsigned short)v<<8;
//    		yv_tmp = v+(unsigned short)y2<<8;
//    		yuyvData[index++]=yv_tmp;


    		yuyvData[index++]=u;
    		yuyvData[index++]=y1;
    		yuyvData[index++]=v;
    		yuyvData[index++]=y2;

//    		memcpy2usb(/*frame,*/ frame_buf, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);

		}

	}





    return TRUE;
}


/*****************************************************************************
  Function:     閿熼叺鍑ゆ嫹閿熸枻鎷烽甯ч敓鏂ゆ嫹閿熺煫绛规嫹閿熸枻鎷烽敓鏂ゆ嫹鍗犻敓闃剁》鎷烽敓鏂ゆ嫹纰岄敓鏂ゆ嫹閿熺嫛鎶碘槄鎷烽敓鏂ゆ嫹閿燂拷
  Description:  閿熼叺鍑ゆ嫹閿熸枻鎷烽甯ч敓鏂ゆ嫹閿熺煫绛规嫹閿熸枻鎷烽敓鏂ゆ嫹鍗犻敓闃剁》鎷烽敓鏂ゆ嫹纰岄敓鏂ゆ嫹閿熺嫛鎶碘槄鎷烽敓鏂ゆ嫹閿燂拷
  Input:        none
  Output:       none
  Return:       none
  Author:       cc
  Modify:
*****************************************************************************/
int free_frame()
{
    if(index_flag != -1)
    {
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = index_flag;

        if(ioctl(fd, VIDIOC_QBUF, &buf) == -1)
        {
            return FALSE;
        }
        return TRUE;
    }
    return FALSE;
}

//
//int main(void)
//{
//
//    FILE * fp2[5];
//    int i;
//    int grab;
//
//	fp2[0] = fopen(YUV0, "wb");
//	fp2[1] = fopen(YUV1, "wb");
//	fp2[2] = fopen(YUV2, "wb");
//	fp2[3] = fopen(YUV3, "wb");
//	fp2[4] = fopen(YUV4, "wb");
//
//	for(i=0;i<5;i++)
//	{
//	    if(!fp2[i])
//		{
//			printf("open error: YUV%d\n",i);
//			return(FALSE);
//		}
//	}
//
//	if(init_v4l2() == FALSE)
//	{
//     	return(FALSE);
//	}
//
////	control_s.id = V4L2_CID_BRIGHTNESS;
////	control_s.value = 80;
////	ioctl(fd, VIDIOC_S_CTRL, &control_s);
//
//    grab=v4l2_grab();
//
//    usleep(1000000);
//
//    for(i=0;i<5*30;i++)
//    {
//
//    	get_frame(&yuv_buffer_pointer,&len);
////    	fwrite(yuv_buffer_pointer, IMAGEHEIGHT*IMAGEWIDTH*2,1, fp2[i]);
//    	fwrite(yuv_buffer_pointer, IMAGEHEIGHT*IMAGEWIDTH*2,1, fp2[0]);//閿熸枻鎷烽敓鏂ゆ嫹鍐�
//
//    	printf("%d\n",buf.index);
//
//    	free_frame();
//
//    }
//
//    printf("GRAB is %d\n",grab);
//    printf("grab ok\n");
//
//    fclose(fp2);
//    close_v4l2();
//
//    return(TRUE);
//}

int  usb_camera_initial()
{

	int grab;


	if(init_v4l2() == FALSE)
	{
		return(FALSE);
	}
	 grab=v4l2_grab();
	 usleep(1000000);
	 printf("GRAB is %d\n",grab);
}

//void with8to16(void**yuv_buffer_pointer,size_t* len,unsigned short yuyvData[IMAGEWIDTH*IMAGEHEIGHT])
//{
//	unsigned int i,j,index=0;
//    unsigned char y1,y2,u,v;
//    char * yuv_buffer_pointer_tmp;
//	unsigned int len_tmp;
//	unsigned short yu_tmp,yv_tmp;
//	get_frame((void **)&yuv_buffer_pointer_tmp,&len_tmp);
//	 printf("get_frame is ok\n");
//	for( i=0; i<IMAGEHEIGHT; i++ )
//	{
//		for( j=0; j<IMAGEWIDTH/2; j++ )
//		{
//    		y1 = *( yuv_buffer_pointer_tmp + (i*IMAGEWIDTH/2+j)*4);
//    		u  = *( yuv_buffer_pointer_tmp + (i*IMAGEWIDTH/2+j)*4 + 1);
//    		y2 = *( yuv_buffer_pointer_tmp + (i*IMAGEWIDTH/2+j)*4 + 2);
//    		v  = *( yuv_buffer_pointer_tmp + (i*IMAGEWIDTH/2+j)*4 + 3);
//
//    		yu_tmp = u+y1<<8;
//    		yuyvData[index++]=yu_tmp;
//    		yv_tmp = v+y2<<8;
//    		yuyvData[index++]=yv_tmp;
//		}
//
//	}
//	 printf("8to16 is ok \n");
//
//}

void data_convert(char*yuv_buffer_pointer_tmp,unsigned short yuyvData[IMAGEWIDTH*IMAGEHEIGHT])
{
	unsigned int i,j,index=0;
    unsigned char y1,y2,u,v;
	unsigned short yu_tmp,yv_tmp;
	for( i=0; i<IMAGEHEIGHT; i++ )
	{
		for( j=0; j<IMAGEWIDTH/2; j++ )
		{
    		y1 = *( yuv_buffer_pointer_tmp + (i*IMAGEWIDTH/2+j)*4);
    		u  = *( yuv_buffer_pointer_tmp + (i*IMAGEWIDTH/2+j)*4 + 1);
    		y2 = *( yuv_buffer_pointer_tmp + (i*IMAGEWIDTH/2+j)*4 + 2);
    		v  = *( yuv_buffer_pointer_tmp + (i*IMAGEWIDTH/2+j)*4 + 3);

    		yu_tmp = u+y1<<8;
    		yuyvData[index++]=yu_tmp;
    		yv_tmp = v+y2<<8;
    		yuyvData[index++]=yv_tmp;
		}

	}


}
